# encoding: utf-8


import matplotlib.pyplot as plt
import numpy as np
import math

# #读文本文件
# file_name = "point_data\WatchPrint5.txt"
# with open(file_name, "r") as f:  # 打开文件
#     data = f.read()  # 读取文件
#     print(data)

plt.rcParams['font.sans-serif']=['SimHei'] # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False # 用来正常显示负号

#码垛结构正解换算
KIN_PI = 3.141592653589793238462643383279
def fun1(a):
    #DH连杆参数
    dh = np.array([0.0,270,880,1250,1400,250,301.6])
    #角度变化
    J1 = a[0]
    J2 = a[1] + 90
    J3 = a[2] + 90
    J4 = a[3]

    a = dh[4]
    c = dh[3]

    cosb = math.cos((J3/180.0)*KIN_PI)
    b = math.sqrt(a*a+c*c-2*a*c*cosb)
    cosa = (b*b+c*c-a*a)/(2*b*c)
    Angle_a = math.acos(cosa)*180.0/KIN_PI
    Angle_a1=J2-Angle_a

    re = np.zeros(8, dtype=np.float)
    VectorA = dh[1]+b*math.cos(Angle_a1*KIN_PI/180)+dh[5]

    re[0] = math.cos(J1*KIN_PI/180.0) * VectorA
    re[1] = math.sin(J1*KIN_PI/180.0) * VectorA
    re[2] = dh[2]+b*math.sin(Angle_a1*KIN_PI/180)-dh[6]
    return re



#使用numpy导入数据

#减速比 轴顺序：1-4-2-3
t1 = [209.4287,148.8375,203.5263,203.5263,209.4287,148.8375,203.5263,203.5263]
t2 = t1
for i in range(8):
    t2[i] = 524288*t1[i]/360
# print(t2)
file_name = "point_data\WatchPrint9.txt"
data = np.loadtxt(file_name, delimiter=',')

data = data/t2

# print(data[0,])

#定义一个空数组，存储关节值
data1 = np.zeros(data.shape)

#关节角度交换、耦合关节换算
i=0
for i in range(data.shape[0]):
    data1[i,0] = data[i,0]
    data1[i,1] = data[i,2]
    data1[i,2] = data[i,3] - data[i,2]
    data1[i,3] = data[i,1]

    data1[i,4] = data[i,4]
    data1[i,5] = data[i,6]
    data1[i,6] = data[i,7] - data[i,6]
    data1[i,7] = data[i,5]

#正解换算
#定义一个空数组，存储笛卡尔位置
data2 = np.zeros(data.shape)
i = 0
for i in range(data.shape[0]):
    data2[i,] = fun1(data[i,])


# print(data2[0,])
 
# print(type(data))
# #数组数据类型
# print(data.dtype)
# #返回数据长度
# print(data.shape)

# t1 = np.arange(10)
# print(t1)
# print(t1.shape[0])
# print(type(t1.shape))
# print(type(t1.shape.count))

file_name1 = "point_data\图一"
plt.figure(file_name1)
ax = plt.subplot(projection = '3d')  # 创建一个三维的绘图工程

ax.set_title('机器人轨迹（毫米）')  # 设置本图名称

# plt.xlim(-700, 20);
# plt.ylim(-1200, -2500);

# plt.xlim(-2500, -1500);
# # plt.ylim(-2500, -800);

# plt.xlim(0, 2000)
# plt.ylim(-3000, 3000)
# plt.zlim(0,1500)

# 机器人实际运动轨迹
# ax.scatter(x, y, z, c = 'r')   # 绘制数据点 c: 'r'红色，'y'黄色，等颜色
# ax.plot(x, y, z, 'rx')   # 绘制数据点 c: 'r'红色，'y'黄色，等颜色
# 机器人理论运动轨迹
x = data2[:,0]
y = data2[:,1]
z = data2[:,2]
ax.plot(x, y, z,c = 'b')
ax.set_xlabel('X')  # 设置x坐标轴
ax.set_ylabel('Y')  # 设置y坐标轴
ax.set_zlabel('Z')  # 设置z坐标轴
plt.show()

